#include <iostream>
#include <fstream>
#include <string>
#include <cmath>
#include <iomanip>

using namespace std;

const double PI = acos(-1.0);

int main (int argc, char *argv[]) {
   if (argc!=3) {
        cout << "***ERROR***: Expect programe to be called with names of two log files.\n";
        return 1;
    }
    ifstream log1(argv[1]);
    ifstream log2(argv[2]);
    if (!log1) {
        cout << "***ERROR***: Cannot open file '" << argv[1] << "' for input\n";
        return 1;
    }
    if (!log2) {
        cout << "***ERROR***: Cannot open file '" << argv[1] << "' for input\n";
        return 1;
    }

    int nAtoms;

    /******************************************
    *
    * READ OPTIMUM GEOMETRY OF MOLECULES
    *
    ******************************************/
    string line, word; // read "line" to get to the right point, word once there
    int geomPos;

    //MOLECULE 1
    while (log1) {
	getline (log1, line);
	if (line.find("NAtoms=")!=string::npos) {
	    nAtoms=atoi((line.substr(8,5)).c_str());
	}
	if (line.find("orientation:")!=string::npos){ 
	    geomPos=log1.tellg();
	}
    }
    log1.clear();		//Clear end of file flags
    log1.seekg(geomPos); 	//Put get pointer at line after last "Standard orientation:"
    getline(log1, word); getline(log1, word); getline(log1, word); getline(log1, word); //Skip 4 lines
    //Put optimum geometry in geom1[atom][X,Y,Z]
    double geom1[nAtoms][3];
    int atomMass[nAtoms];
    for (int i=0; i<nAtoms; i++) {
	log1 >> word; 
	log1 >> word;   atomMass[i]=atoi(word.c_str());
	if (atomMass[i]!=1 && atomMass[i]!=6) {cerr << "***ERROR***: Found atom that isn't C or H\n"; return -1;}
	if (atomMass[i]==1) atomMass[i]=1;
	if (atomMass[i]==6) atomMass[i]=12;
	log1 >> word; 
	log1 >> word; 	geom1[i][0]=atof(word.c_str());
	log1 >> word; 	geom1[i][1]=atof(word.c_str());
	log1 >> word; 	geom1[i][2]=atof(word.c_str());
    }
    //MOLECULE 2
    while (log2) {
	getline (log2, line);
	if (line.find("NAtoms=")!=string::npos) {
	    if (atoi((line.substr(8,5)).c_str())!=nAtoms) {
		cerr << "***ERROR***: Different number of atoms in two log files presented.\n";
		return -1;
	    }
	}
	if (line.find("orientation:")!=string::npos){ 
	    geomPos=log2.tellg();
	}
    }
    log2.clear();	
    log2.seekg(geomPos);
    getline(log2, word); getline(log2, word); getline(log2, word); getline(log2, word); //Skip 4 lines
    double geom2[nAtoms][3];
    for (int i=0; i<nAtoms; i++) {
	log2 >> word; log2 >> word; log2 >> word; log2 >> word; 	
        geom2[i][0]=atof(word.c_str());
	log2 >> word; 		 
        geom2[i][1]=atof(word.c_str());
	log2 >> word; 		
        geom2[i][2]=atof(word.c_str());
    }

    /************************************************************************
    * FIND NORMAL MODE OUTPUT
    * 	-Have to include scale factor for systematic errors in HF frequencies
    *		(see J. Phys. Chem. 100, 41, 16505)
    *************************************************************************/
    cout << "\nWhat level of theory? 1=am1, 2=HF/6-31G(d) 3=B3LYP/6-31G* 4=no_scaling\n";
    int theory;
    cin >> theory;
    double scaling;
    switch(theory) {
    	case 1:
	    cout << "For am1 applying a frequency scaling factor of 0.9352\n";
	    scaling = 0.9532;
	    break;
	case 2:
	    cout << "For HF/6-31G(d) applying a frequency scaling factor of 0.8929\n";
	    scaling = 0.8929;
	    break;
	case 3: 
	    cout << "For B3lyp/6-31g* applying a frequency scaling factor of 0.9613\n";
	    scaling = 0.9613;
	    break;
	case 4:
	    cout << "Frequency scaling = 1\n";
	    scaling = 1.0;
	    break;
	default:
	    cout << "\n***ERROR***: Didn't understand which level of theory you want\n";
	    return -1;
    }
    double freq1[3*nAtoms-6];
    double mass1[3*nAtoms-6];
    double NMcoord1[3*nAtoms-6][nAtoms][3];
    int freqRead=0;			//Keep track of frequencies read
    int massRead=0;			//       "      masses read
    int NMcoordRead=0;			//       "      normal mode coordinates read
    int NM_added;
    //Should not have gone beyond the NM data.
    while (log1 && NMcoordRead<3*nAtoms-6) { 
	getline(log1, line);
	if (line.find("Frequencies --")!=string::npos) {
	    if (freqRead<3*nAtoms-6) {freq1[freqRead]=scaling*atof(line.substr(23,10).c_str()); freqRead++;}
	    if (freqRead<3*nAtoms-6) {freq1[freqRead]=scaling*atof(line.substr(33,10).c_str()); freqRead++;}
	    if (freqRead<3*nAtoms-6) {freq1[freqRead]=scaling*atof(line.substr(43,10).c_str()); freqRead++;}
	    if (freqRead<3*nAtoms-6) {freq1[freqRead]=scaling*atof(line.substr(53,10).c_str()); freqRead++;}
	    if (freqRead<3*nAtoms-6) {freq1[freqRead]=scaling*atof(line.substr(63,10).c_str()); freqRead++;}
	    if (freq1[freqRead-5]<0.0 || freq1[freqRead-4]<0.0 || freq1[freqRead-3]<0.0 || freq1[freqRead-2]<0.0 || freq1[freqRead-1]<0.0) {
		cerr << "***ERROR***: Found imaginary frequencies.  You do not have a stable geometry\n";
		cerr << freq1[freqRead-4] << " " << freq1[freqRead-3] << " " << freq1[freqRead-2] << " " << freq1[freqRead-1] 
		     << freq1[freqRead] << endl;
		return 1;
	    }
	}
	if (line.find("Reduced masses ---")!=string::npos) {
	    if (massRead<3*nAtoms-6) {mass1[massRead]=atof(line.substr(23,10).c_str()); massRead++;}
	    if (massRead<3*nAtoms-6) {mass1[massRead]=atof(line.substr(33,10).c_str()); massRead++;}
	    if (massRead<3*nAtoms-6) {mass1[massRead]=atof(line.substr(43,10).c_str()); massRead++;}
	    if (massRead<3*nAtoms-6) {mass1[massRead]=atof(line.substr(53,10).c_str()); massRead++;}
	    if (massRead<3*nAtoms-6) {mass1[massRead]=atof(line.substr(63,10).c_str()); massRead++;}
	}
	if (line.find("Coord Atom Element:")!=string::npos) {
	    for (int i=0; i<nAtoms; i++) {
		for (int j=0; j<3; j++) {
		    getline(log1,line);
		    if (NMcoordRead  <3*nAtoms-6) {NMcoord1[NMcoordRead]  [i][j]=atof(line.substr(25,8).c_str()); NM_added=1;}
		    if (NMcoordRead+1<3*nAtoms-6) {NMcoord1[NMcoordRead+1][i][j]=atof(line.substr(35,8).c_str()); NM_added=2;}
		    if (NMcoordRead+2<3*nAtoms-6) {NMcoord1[NMcoordRead+2][i][j]=atof(line.substr(45,8).c_str()); NM_added=3;}
		    if (NMcoordRead+3<3*nAtoms-6) {NMcoord1[NMcoordRead+3][i][j]=atof(line.substr(55,8).c_str()); NM_added=4;}
		    if (NMcoordRead+4<3*nAtoms-6) {NMcoord1[NMcoordRead+4][i][j]=atof(line.substr(65,8).c_str()); NM_added=5;}
		}
	    }
	    NMcoordRead+=NM_added;
	}
    }
    if (freqRead==0) {
	cerr << "***ERROR***: Didn't find any normal mode frequencies\n";
	return -1;
    }
    if (freqRead!=massRead || freqRead!=NMcoordRead) {
	cerr << "***ERROR***: Error in reading Normal Mode data\n"
	     << "             Read " << freqRead << " frequencies, " << massRead << " masses and " << NMcoordRead << " NM coords\n";
	return -1;
    }
    /**************************************************************
    * CALCULATE MASS WEIGHTED DISTORTION AND NORMAL MODES 
    * N.B. normal modes in cartesian coordinates are not orthogonal
    **************************************************************/
    double geomDiffMag=0.0;
    double geomDiff[nAtoms][3];
    for (int i=0; i<nAtoms; i++) {
	for (int j=0; j<3; j++) {
	    geomDiff[i][j]=sqrt(atomMass[i]*1.660538863e-27)*(geom1[i][j]-geom2[i][j]); //Ang.kg^0.5
	    geomDiffMag+=geomDiff[i][j]*geomDiff[i][j];
	}
    }
    double mwNM[3*nAtoms-6][nAtoms][3];  //mass weighted normal modes
    for (int i=0; i<3*nAtoms-6; i++) {
	double normSqd=0.0;
	for (int j=0; j<nAtoms; j++) {
	    for (int k=0; k<3; k++) {
		mwNM[i][j][k]=sqrt(atomMass[j])*NMcoord1[i][j][k]; //mass weight
		normSqd+=mwNM[i][j][k]*mwNM[i][j][k];
	    }
	}
	for (int j=0; j<nAtoms; j++) {
	    for (int k=0; k<3; k++) {
		mwNM[i][j][k]/=sqrt(normSqd);	//normalise
	    }
	}
    }
    /******************************************
    * DECOMPOSE INTO MASS WEIGHTED NORMAL MODES
    ******************************************/
    double reorgNM_Disp[3*nAtoms-6];	//Displacement along each mass weighted (normalised) normal mode
    double reorgNM_Energy[3*nAtoms-6];  //Energy along -"-
    double reorgNM_DispMag=0.0;
    double reorgTotal=0.0;
    for (int i=0; i<3*nAtoms-6; i++) {
	reorgNM_Disp[i]  =0.0;
	reorgNM_Energy[i]=0.0;
    }
    for (int i=0; i<3*nAtoms-6; i++) {
	for (int j=0; j<nAtoms; j++) {
	    for (int k=0; k<3; k++) {
		reorgNM_Disp[i]+=mwNM[i][j][k]*geomDiff[j][k];
	    }
	}
	reorgNM_Energy[i]=2*PI*PI*freq1[i]*freq1[i]*8.987551787*reorgNM_Disp[i]*reorgNM_Disp[i]*6.24150634e18;
	//1/2 (1/2 kx^2) 4PI -> radial freq; 8.98e20 cm^-1 -> Hz; 1e-20 Ang^2 -> m^2; 6.2e18 J -> eV
	reorgTotal+=reorgNM_Energy[i];
	reorgNM_DispMag+=reorgNM_Disp[i]*reorgNM_Disp[i];
    }
    cout << "Decomposition of lambda into NM (frequency (cm^-1);  Displacement(M.W.! Q/sqrt(\\mu) -> X)   Energy (meV);  \t contribution.\n";
    for (int i=0; i<3*nAtoms-6; i++) {
	cout << i << '\t' << freq1[i] << '\t' << reorgNM_Disp[i] << '\t'
         << reorgNM_Energy[i] << '\t' << reorgNM_Energy[i]/reorgTotal << endl;
    }
    cout << "reorgTotal (meV) = " << 1e3*reorgTotal << endl;
    cout <<"geomDiffMag (mw)="<<sqrt(geomDiffMag) << "   reorgNM_DispMag (mw) =" << sqrt(reorgNM_DispMag)
         << "   geomDiffMag/reorgNM_DispMag=" << sqrt(geomDiffMag/reorgNM_DispMag) << endl;
    
    //Print geometries
    /*
    for (int i=0; i<nAtoms; i++) {
	for (int j=0; j<3; j++) {
	    cout << setprecision (7) << geom1[i][j] << '\t';
	}
	cout << endl;
    }
    */
    //Print Modes
    /*
    for (int i=0; i<3*nAtoms-6; i++) {
	cout << "Mode " << i << endl;
	for (int j=0; j<nAtoms; j++) {
	    cout << NMcoord1[i][j][0] << " " << NMcoord1[i][j][1] << " "<< NMcoord1[i][j][2] << endl; 
	}
    }
    */
    /*
    //Test normal modes are othogonal 
    double mwNM[3*nAtoms-6][nAtoms][3];  //mass weighted normal modes
    for (int i=0; i<3*nAtoms-6; i++) {
	double normSqd=0.0;
	for (int j=0; j<nAtoms; j++) {
	    for (int k=0; k<3; k++) {
		mwNM[i][j][k]=sqrt(atomMass[j])*NMcoord1[i][j][k];
		normSqd+=mwNM[i][j][k]*mwNM[i][j][k];
	    }
	}
	for (int j=0; j<nAtoms; j++) {
	    for (int k=0; k<3; k++) {
		mwNM[i][j][k]/=sqrt(normSqd);
	    }
	}
    }
    for (int i=0; i<3*nAtoms-7; i++) {
	for (int j=i+1; j<3*nAtoms-6; j++) {
	    double dotProdSqd=0.0;
	    double errorSqd=0.0;
	    for (int k=0; k<nAtoms; k++) {
		for (int l=0; l<3; l++) {
		    dotProdSqd+=mwNM[i][k][l]*mwNM[j][k][l];
		    if (mwNM[i][k][l]>0.0) errorSqd+=(5e-6/mwNM[i][k][l])*(5e-6/mwNM[i][k][l]);
		    if (mwNM[j][k][l]>0.0) errorSqd+=(5e-6/mwNM[j][k][l])*(5e-6/mwNM[j][k][l]);
		}
	    }
	    if (acos(dotProdSqd)*180/PI<80) {
		cout << "Angle between mwNM_" << i << "and mwNM_" << j << " = " << acos(dotProdSqd)*180/PI << endl 
		     << "\tLower estimation of  error (when mw[i][k][l]<1e-5 disregard contribution)= " << sqrt(errorSqd)  << endl
	             << "\tLower range in angles = " << acos(dotProdSqd+sqrt(errorSqd))*180/PI << " to " << acos(dotProdSqd-sqrt(errorSqd))*180/PI << endl;
	    }
	}
    }
    */
    //Print frequencies
    /*
    cout << fixed << scientific;
    cout << "f/cm-1\t\tf/Hz\t\tk/N.m^-1" << endl;
    for (int i=0; i<3*nAtoms-6; i++) {
	cout << freq1[i] << "\t" << freq1[i]*2.99792458e10 << "\t" 
	     << 4*PI*PI*1.660538863e-27*mass1[i]*freq1[i]*freq1[i]*8.987551787e20 << endl;
    }
    return 0; //End of main()
    */
}

    
